#include "motor_controller.h"
#include "state_manager.h"

#include <mutex>

#include "ws_constant.h"
#include "ws_response.h"
#include "elog.h"
#include "elog_file.h"

using namespace std;

std::mutex motor_controller_mutex;

static StateManager* state_manager = StateManager::GetInstance();

static unordered_map<int, int (MotorController::*)(WsPacket& ws_packet)> motor_func_table {
    { CMD_STEP_MOTOR_RUN, &MotorController::run },
    { CMD_STEP_MOTOR_RUN_TO, &MotorController::runTo },
    { CMD_STEP_MOTOR_STOP, &MotorController::stop },
    { CMD_STEP_MOTOR_RESET, &MotorController::reset },
    { CMD_STEP_MOTOR_CHANGE_SPEED, &MotorController::changeSpeed },
    { CMD_STEP_MOTOR_CHANGE_DIRECTION, &MotorController::changeDirection },
    { CMD_STEP_MOTOR_SERVICE_JOYSTICK, &MotorController::serviceJoystick },
    { CMD_STEP_MOTOR_SERVICE_JOYSTICK_FIXED_ANGLE, &MotorController::serviceJoystickFixedAngle },
    { CMD_STEP_MOTOR_SERVICE_JOYSTICK_STOP, &MotorController::serviceJoystickStop },
    { CMD_STEP_MOTOR_SERVICE_DUAL_CAMERA_LINKAGE, &MotorController::startDualCameraLinkage }
};

int MotorController::handleMessage(WsPacket& ws_packet) {
    // log_i("handle motor message, cmd = %d", ws_packet.cmd());

    //在表中查找函数指针并调用
    auto it = motor_func_table.find(ws_packet.cmd());
    if (it != motor_func_table.end()) {
        (this->*(it->second))(ws_packet);
    }
    else {
        log_i("Function not found");
    }

    return 0;
}

int MotorController::serviceJoystick(WsPacket& ws_packet) {
    ReqMotorServiceJoystick req_motor_service_joystick;
    string serialized_data = ws_packet.data();

    if (!req_motor_service_joystick.ParseFromString(serialized_data)) {
        return -WS_PARSE_PROTOBUF_ERROR;
    }

    double angle = req_motor_service_joystick.vector_angle();
    double length = req_motor_service_joystick.vector_length();
    double speed = req_motor_service_joystick.speed();

    // log_i("angle:%f, length:%f, speed:%f", angle, length, speed);

    if (!motor_controller_mutex.try_lock()) {
        log_e("INVALID CMD: motor busy!");
        return -1;
    }

    std::lock_guard<std::mutex> guard(motor_controller_mutex, std::adopt_lock);

    double speed_combined = length * speed;
    double speed_yaw = speed_combined * COSD(angle);
    double speed_pitch = speed_combined * SIND(angle);

    bool dir_yaw = speed_yaw < 0 ? YAW_LEFT : YAW_RIGHT;
    bool dir_pitch = speed_pitch < 0 ? PITCH_DOWN : PITCH_UP;

    speed_yaw = abs(speed_yaw);
    speed_pitch = abs(speed_pitch);

    const double speed_ramping_time = 0.3;

    bool now_dir_yaw = motorYaw().getDirection();
    if (now_dir_yaw != dir_yaw)
        motorYaw().stop();

    int ret = motorYaw().setParameterInDegree(speed_yaw, STEP_MOTOR_MINISTEP_8, speed_ramping_time, dir_yaw);
    if (ret == -CODE_STEP_MOTOR_IS_RUNNING) {
        motorYaw().changeSpeed(speed_yaw);
        // motorYaw().changeDirection(dir_yaw, true);
    } else if (ret < 0) {
        wsCommonResponse(CMD_STEP_MOTOR_SERVICE_JOYSTICK, MODULE_MOTOR, CMD_TYPE_RESPONSE, -WS_INVAID_PARAM);
        return ret;
    } else {
        motorYaw().run();
    }

    bool now_dir_pitch = motorPitch().getDirection();
    if (now_dir_pitch != dir_pitch)
        motorPitch().stop();

    ret = motorPitch().setParameterInDegree(speed_pitch, STEP_MOTOR_MINISTEP_8, speed_ramping_time, dir_pitch);
    if (ret == -CODE_STEP_MOTOR_IS_RUNNING) {
        motorPitch().changeSpeed(speed_pitch);
        // motorPitch().changeDirection(dir_pitch);
    } else if (ret < 0) {
        wsCommonResponse(CMD_STEP_MOTOR_SERVICE_JOYSTICK, MODULE_MOTOR, CMD_TYPE_RESPONSE, -WS_INVAID_PARAM);
        return ret;
    } else {
        motorPitch().run();
    }

    wsCommonResponse(CMD_STEP_MOTOR_SERVICE_JOYSTICK, MODULE_MOTOR, CMD_TYPE_RESPONSE, WS_OK);

    return 0;
}

int MotorController::serviceJoystickFixedAngle(WsPacket& ws_packet) {
    ReqMotorServiceJoystickFixedAngle req_motor_service_joystick_fixed_angle;
    string serialized_data = ws_packet.data();

    if (!req_motor_service_joystick_fixed_angle.ParseFromString(serialized_data)) {
        return -WS_PARSE_PROTOBUF_ERROR;
    }

    double angle = req_motor_service_joystick_fixed_angle.vector_angle();
    double length = req_motor_service_joystick_fixed_angle.vector_length();
    double speed = req_motor_service_joystick_fixed_angle.speed();

    // log_i("angle:%f, length:%f, speed:%f", angle, length, speed);

    if (!motor_controller_mutex.try_lock()) {
        log_e("INVALID CMD: motor busy!");
        return -1;
    }

    std::lock_guard<std::mutex> guard(motor_controller_mutex, std::adopt_lock);

    double speed_combined = length * speed;
    double speed_yaw = speed_combined * COSD(angle);
    double speed_pitch = speed_combined * SIND(angle);

    bool dir_yaw = speed_yaw < 0 ? YAW_LEFT : YAW_RIGHT;
    bool dir_pitch = speed_pitch < 0 ? PITCH_DOWN : PITCH_UP;

    speed_yaw = abs(speed_yaw);
    speed_pitch = abs(speed_pitch);

    const double rotate_angle = 0.3;
    const double speed_ramping_time = 0.3;

    int ret = motorYaw().setParameterInDegree(rotate_angle, speed_yaw, STEP_MOTOR_MINISTEP_8, speed_ramping_time, dir_yaw);
    if (ret < 0) {
        wsCommonResponse(CMD_STEP_MOTOR_SERVICE_JOYSTICK_FIXED_ANGLE, MODULE_MOTOR, CMD_TYPE_RESPONSE, -WS_INVAID_PARAM);
        return ret;
    } else {
        motorYaw().run();
    }

    ret = motorPitch().setParameterInDegree(rotate_angle, speed_pitch, STEP_MOTOR_MINISTEP_8, speed_ramping_time, dir_pitch);
    if (ret < 0) {
        wsCommonResponse(CMD_STEP_MOTOR_SERVICE_JOYSTICK_FIXED_ANGLE, MODULE_MOTOR, CMD_TYPE_RESPONSE, -WS_INVAID_PARAM);
        return ret;
    } else {
        motorPitch().run();
    }

    wsCommonResponse(CMD_STEP_MOTOR_SERVICE_JOYSTICK_FIXED_ANGLE, MODULE_MOTOR, CMD_TYPE_RESPONSE, WS_OK);

    return 0;
}

int MotorController::serviceJoystickStop(WsPacket& ws_packet) {
    std::lock_guard<std::mutex> guard(motor_controller_mutex);

    motorYaw().stop();
    motorPitch().stop();

    wsCommonResponse(CMD_STEP_MOTOR_SERVICE_JOYSTICK_STOP, MODULE_MOTOR, CMD_TYPE_RESPONSE, WS_OK);

    return 0;
}

int MotorController::run(WsPacket& ws_packet) {
    ReqMotorRun req_motor_run;
    string serialized_data = ws_packet.data();

    if (!req_motor_run.ParseFromString(serialized_data)) {
        return -WS_PARSE_PROTOBUF_ERROR;
    }

    StepMotorUser* motor = getMotor(req_motor_run.id());
    if (motor == nullptr) {
        wsResMotor(CMD_STEP_MOTOR_RUN, MODULE_MOTOR, CMD_TYPE_RESPONSE, req_motor_run.id(), -CODE_STEP_MOTOR_INVALID_PARAMETER_ID);
        return -CODE_STEP_MOTOR_INVALID_PARAMETER_ID;
    }

    int ret = motor->setParameterInDegree(req_motor_run.speed(), req_motor_run.resolution_level(), req_motor_run.speed_ramping(), req_motor_run.direction());
    if (ret == -CODE_STEP_MOTOR_IS_RUNNING) {
        motor->changeSpeed(req_motor_run.speed());
        motor->changeDirection(req_motor_run.direction());

        wsResMotor(CMD_STEP_MOTOR_RUN, MODULE_MOTOR, CMD_TYPE_RESPONSE, req_motor_run.id(), 0);
        return -ret;
    } else if (ret < 0) {
        wsResMotor(CMD_STEP_MOTOR_RUN, MODULE_MOTOR, CMD_TYPE_RESPONSE, req_motor_run.id(), ret);
        return ret;
    }

    motor->run();

    wsResMotor(CMD_STEP_MOTOR_RUN, MODULE_MOTOR, CMD_TYPE_RESPONSE, req_motor_run.id(), ret);

    return 0;
}

int MotorController::runTo(WsPacket& ws_packet) {
    ReqMotorRunTo req_motor_run_to;
    string serialized_data = ws_packet.data();

    if (!req_motor_run_to.ParseFromString(serialized_data)) {
        return -WS_PARSE_PROTOBUF_ERROR;
    }
    log_i("motor runTo id = %d", req_motor_run_to.id());
    StepMotorUser* motor = getMotor(req_motor_run_to.id());
    if (motor == nullptr) {
        wsResMotor(CMD_STEP_MOTOR_RUN_TO, MODULE_MOTOR, CMD_TYPE_RESPONSE, req_motor_run_to.id(), -CODE_STEP_MOTOR_INVALID_PARAMETER_ID);
        return -CODE_STEP_MOTOR_INVALID_PARAMETER_ID;
    }

    int ret = motor->setParameterInPositionInDegree(req_motor_run_to.end_position(), req_motor_run_to.speed(), req_motor_run_to.resolution_level(), 
                                                    req_motor_run_to.speed_ramping());
    if (ret < 0) {
        wsResMotor(CMD_STEP_MOTOR_RUN_TO, MODULE_MOTOR, CMD_TYPE_RESPONSE, req_motor_run_to.id(), ret);
        return ret;
    }

    motor->run();

    ret = motor->waitMotorToStop();

    wsResMotor(CMD_STEP_MOTOR_RUN_TO, MODULE_MOTOR, CMD_TYPE_RESPONSE, req_motor_run_to.id(), ret);

    return ret;
}

int MotorController::stop(WsPacket& ws_packet) {
    ReqMotorStop req_motor_stop;
    string serialized_data = ws_packet.data();

    if (!req_motor_stop.ParseFromString(serialized_data)) {
        return -WS_PARSE_PROTOBUF_ERROR;
    }

    StepMotorUser* motor = getMotor(req_motor_stop.id());
    if (motor == nullptr) {
        wsResMotor(CMD_STEP_MOTOR_STOP, MODULE_MOTOR, CMD_TYPE_RESPONSE, req_motor_stop.id(), -CODE_STEP_MOTOR_INVALID_PARAMETER_ID);
        return -CODE_STEP_MOTOR_INVALID_PARAMETER_ID;
    }

    int ret = motor->stop();

    wsResMotor(CMD_STEP_MOTOR_STOP, MODULE_MOTOR, CMD_TYPE_RESPONSE, req_motor_stop.id(), ret);

    return ret;
}

int MotorController::reset(WsPacket& ws_packet) {
    ReqMotorReset req_motor_reset;
    string serialized_data = ws_packet.data();

    if (!req_motor_reset.ParseFromString(serialized_data)) {
        return -WS_PARSE_PROTOBUF_ERROR;
    }
    log_i("motor reset id = %d", req_motor_reset.id());
    StepMotorUser* motor = getMotor(req_motor_reset.id());
    if (motor == nullptr) {
        wsResMotor(CMD_STEP_MOTOR_RESET, MODULE_MOTOR, CMD_TYPE_RESPONSE, req_motor_reset.id(), -CODE_STEP_MOTOR_INVALID_PARAMETER_ID);
        return -CODE_STEP_MOTOR_INVALID_PARAMETER_ID;
    }

    int ret = motor->resetAndRunInPulse(req_motor_reset.direction());
    if (ret < 0) {
        wsResMotor(CMD_STEP_MOTOR_RESET, MODULE_MOTOR, CMD_TYPE_RESPONSE, req_motor_reset.id(), ret);
        return ret;
    }

    double position = 0;
    ret = motor->getAbsolutePositionInDegree(STEP_MOTOR_MINISTEP_32, position);
    if (ret < 0) {
        wsResMotor(CMD_STEP_MOTOR_RESET, MODULE_MOTOR, CMD_TYPE_RESPONSE, req_motor_reset.id(), ret);
        return ret;
    }

    wsResMotorPosition(CMD_STEP_MOTOR_RESET, MODULE_MOTOR, CMD_TYPE_RESPONSE, req_motor_reset.id(), ret, position);

    return ret;
}

int MotorController::changeSpeed(WsPacket& ws_packet) {
    ReqMotorChangeSpeed req_motor_change_speed;
    string serialized_data = ws_packet.data();

    if (!req_motor_change_speed.ParseFromString(serialized_data)) {
        return -WS_PARSE_PROTOBUF_ERROR;
    }

    StepMotorUser* motor = getMotor(req_motor_change_speed.id());
    if (motor == nullptr) {
        wsResMotor(CMD_STEP_MOTOR_CHANGE_SPEED, MODULE_MOTOR, CMD_TYPE_RESPONSE, req_motor_change_speed.id(), -CODE_STEP_MOTOR_INVALID_PARAMETER_ID);
        return -CODE_STEP_MOTOR_INVALID_PARAMETER_ID;
    }

    int ret = motor->changeSpeed(req_motor_change_speed.speed());
    if (ret < 0) {
        wsResMotor(CMD_STEP_MOTOR_CHANGE_SPEED, MODULE_MOTOR, CMD_TYPE_RESPONSE, req_motor_change_speed.id(), ret);
        return ret;
    }

    wsResMotor(CMD_STEP_MOTOR_CHANGE_SPEED, MODULE_MOTOR, CMD_TYPE_RESPONSE, req_motor_change_speed.id(), ret);

    return ret;
}

int MotorController::changeDirection(WsPacket& ws_packet) {
    ReqMotorChangeDirection req_motor_change_direction;
    string serialized_data = ws_packet.data();

    if (!req_motor_change_direction.ParseFromString(serialized_data)) {
        return -WS_PARSE_PROTOBUF_ERROR;
    }

    StepMotorUser* motor = getMotor(req_motor_change_direction.id());
    if (motor == nullptr) {
        wsResMotor(CMD_STEP_MOTOR_CHANGE_DIRECTION, MODULE_MOTOR, CMD_TYPE_RESPONSE, req_motor_change_direction.id(), -CODE_STEP_MOTOR_INVALID_PARAMETER_ID);
        return -CODE_STEP_MOTOR_INVALID_PARAMETER_ID;
    }

    int ret = motor->changeDirection(req_motor_change_direction.direction());
    if (ret < 0) {
        wsResMotor(CMD_STEP_MOTOR_CHANGE_DIRECTION, MODULE_MOTOR, CMD_TYPE_RESPONSE, req_motor_change_direction.id(), ret);
        return ret;
    }

    wsResMotor(CMD_STEP_MOTOR_CHANGE_DIRECTION, MODULE_MOTOR, CMD_TYPE_RESPONSE, req_motor_change_direction.id(), ret);

    return ret;
}

StepMotorUser& MotorController::motorYaw() {
    static StepMotorUser motor_yaw(MOTOR_YAW, motor_yaw_gear_ratio_, motor_yaw_min_frequency_, motor_yaw_max_frequency_,  
                                motor_yaw_hires_max_frequency_, motor_yaw_min_soft_limit_, motor_yaw_max_soft_limit_, 
                                motor_yaw_step_angle_, motor_yaw_reset_time_s_, motor_yaw_backlash_);
    return motor_yaw;
}

StepMotorUser& MotorController::motorPitch() {
    static StepMotorUser motor_pitch(MOTOR_PITCH, motor_pitch_gear_ratio_, motor_yaw_min_frequency_, motor_pitch_max_frequency_, 
                                    motor_pitch_hires_max_frequency_, motor_pitch_min_soft_limit_, motor_pitch_max_soft_limit_, 
                                    motor_pitch_step_angle_, motor_pitch_reset_time_s_, motor_pitch_backlash_);
    return motor_pitch;
}

StepMotorUser& MotorController::motorFocus() {
    static StepMotorUser motor_focus(MOTOR_FOCUS, motor_focus_gear_ratio_, motor_yaw_min_frequency_, motor_focus_max_frequency_, 
                                    motor_focus_hires_max_frequency_, motor_focus_min_soft_limit_, motor_focus_max_soft_limit_, 
                                    motor_focus_step_angle_, motor_focus_reset_time_s_, motor_focus_backlash_);
    return motor_focus;
}

void MotorController::initMotorController() {
    motorYaw();
    motorPitch();
    motorFocus();

    StepMotorDriver::initMotor();
}

StepMotorUser* MotorController::getMotor(int id) {
    switch (id) {
    case MOTOR_YAW: {
        StepMotorUser& motor = motorYaw();
        return &motor;
    }

    case MOTOR_PITCH: {
        StepMotorUser& motor = motorPitch();
        return &motor; 
    }

    case MOTOR_FOCUS: {
        StepMotorUser& motor = motorFocus();
        return &motor; 
    }
    
    default:
        return nullptr;
    }
}


int MotorController::startDualCameraLinkage(cv::Point pos) {
    int x = pos.x;
    int y = pos.y;
    x = x * 1920.0 / 1280;
    y = y * 1080.0 / 720;

    log_i("startDualCameraLinkage x = %d, y = %d", x, y);

    cv::Point point_wide_view(x, y);
    HorizontalCoordinates point_wide_view_mechanical_coord;
    HorizontalCoordinates center_mechanical_coord_wide_view = {0};

    wideViewPixelCoordToMechanicalCoord(point_wide_view, point_wide_view_mechanical_coord, center_mechanical_coord_wide_view);

    templateRect template_rect = state_manager->focus_.getCurTemplateRect();
    int temp_x = template_rect.left_top_w + template_rect.width / 2;
    int temp_y = template_rect.left_top_h + template_rect.height / 2;
    temp_x = temp_x * 1920.0 / 1280;
    temp_y = temp_y * 1080.0 / 720;
     log_i("startDualCameraLinkage temp_x = %d, temp_y = %d", temp_x, temp_y);
    cv::Point point_wide_template_rect(temp_x, temp_y);
    HorizontalCoordinates point_template_rect_mechanical_coord;

    wideViewPixelCoordToMechanicalCoord(point_wide_template_rect, point_template_rect_mechanical_coord, center_mechanical_coord_wide_view);

    double yaw_angle = point_wide_view_mechanical_coord.azi - point_template_rect_mechanical_coord.azi;
    log_i("point_wide_view_mechanical_coord azi = %.f, point_template_rect_mechanical_coord azi = %.f", point_wide_view_mechanical_coord.azi, point_template_rect_mechanical_coord.azi);
    bool yaw_direction = (x - temp_x) > 0 ? YAW_RIGHT : YAW_LEFT;

    yaw_angle = fabs(yaw_angle) > 180 ? 360 - fabs(yaw_angle) : fabs(yaw_angle); 
    motorYaw().setParameterInDegree(yaw_angle, 20.0, STEP_MOTOR_MINISTEP_8, STEP_MOTOR_RAMP_PULSE_0, yaw_direction);
    motorYaw().run();

    double pitch_angle = point_wide_view_mechanical_coord.alt - point_template_rect_mechanical_coord.alt;
    log_i("point_wide_view_mechanical_coord alt = %.f, point_template_rect_mechanical_coord alt = %.f", point_wide_view_mechanical_coord.alt, point_template_rect_mechanical_coord.alt);
    bool pitch_direction = (y - temp_y) > 0 ? PITCH_DOWN : PITCH_UP;

    motorPitch().setParameterInDegree(fabs(pitch_angle), 20.0, STEP_MOTOR_MINISTEP_8, STEP_MOTOR_RAMP_PULSE_0, pitch_direction);
    motorPitch().run();

    int motor_yaw_ret = motorYaw().waitMotorToStop();
    int motor_pitch_ret = motorPitch().waitMotorToStop();
    
    return 0;
}


int MotorController::startDualCameraLinkage(WsPacket& ws_packet) {
    ReqDualCameraLinkage req_dual_camera_linkage;
    string serialized_data = ws_packet.data();

    if (!req_dual_camera_linkage.ParseFromString(serialized_data)) {
        return -WS_PARSE_PROTOBUF_ERROR;
    }

    int x = req_dual_camera_linkage.x();
    int y = req_dual_camera_linkage.y();
    x = x * 1920.0 / 1280;
    y = y * 1080.0 / 720;

    log_i("startDualCameraLinkage x = %d, y = %d", x, y);

    cv::Point point_wide_view(x, y);
    HorizontalCoordinates point_wide_view_mechanical_coord;
    HorizontalCoordinates center_mechanical_coord_wide_view = {0};

    wideViewPixelCoordToMechanicalCoord(point_wide_view, point_wide_view_mechanical_coord, center_mechanical_coord_wide_view);

    templateRect template_rect = state_manager->focus_.getCurTemplateRect();
    int temp_x = template_rect.left_top_w + template_rect.width / 2;
    int temp_y = template_rect.left_top_h + template_rect.height / 2;
    temp_x = temp_x * 1920.0 / 1280;
    temp_y = temp_y * 1080.0 / 720;
     log_i("startDualCameraLinkage temp_x = %d, temp_y = %d", temp_x, temp_y);
    cv::Point point_wide_template_rect(temp_x, temp_y);
    HorizontalCoordinates point_template_rect_mechanical_coord;

    wideViewPixelCoordToMechanicalCoord(point_wide_template_rect, point_template_rect_mechanical_coord, center_mechanical_coord_wide_view);

    double yaw_angle = point_wide_view_mechanical_coord.azi - point_template_rect_mechanical_coord.azi;
    log_i("point_wide_view_mechanical_coord azi = %.f, point_template_rect_mechanical_coord azi = %.f", point_wide_view_mechanical_coord.azi, point_template_rect_mechanical_coord.azi);
    bool yaw_direction = (x - temp_x) > 0 ? YAW_RIGHT : YAW_LEFT;

    yaw_angle = fabs(yaw_angle) > 180 ? 360 - fabs(yaw_angle) : fabs(yaw_angle); 
    motorYaw().setParameterInDegree(yaw_angle, 20.0, STEP_MOTOR_MINISTEP_8, STEP_MOTOR_RAMP_PULSE_0, yaw_direction);
    motorYaw().run();

    double pitch_angle = point_wide_view_mechanical_coord.alt - point_template_rect_mechanical_coord.alt;
    log_i("point_wide_view_mechanical_coord alt = %.f, point_template_rect_mechanical_coord alt = %.f", point_wide_view_mechanical_coord.alt, point_template_rect_mechanical_coord.alt);
    bool pitch_direction = (y - temp_y) > 0 ? PITCH_DOWN : PITCH_UP;

    motorPitch().setParameterInDegree(fabs(pitch_angle), 20.0, STEP_MOTOR_MINISTEP_8, STEP_MOTOR_RAMP_PULSE_0, pitch_direction);
    motorPitch().run();

    wsResNotifyCamFunctionState(CMD_NOTIFY_TELE_FUNCTION_STATE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, CMD_STEP_MOTOR_SERVICE_DUAL_CAMERA_LINKAGE, STATE_RUNNING);

    int motor_yaw_ret = motorYaw().waitMotorToStop();
    int motor_pitch_ret = motorPitch().waitMotorToStop();

    if (motor_yaw_ret == -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED || motor_pitch_ret == -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED)
        wsCommonResponse(CMD_STEP_MOTOR_SERVICE_DUAL_CAMERA_LINKAGE, MODULE_MOTOR, CMD_TYPE_RESPONSE, -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED);
    else
        wsCommonResponse(CMD_STEP_MOTOR_SERVICE_DUAL_CAMERA_LINKAGE, MODULE_MOTOR, CMD_TYPE_RESPONSE, 0);

    wsResNotifyCamFunctionState(CMD_NOTIFY_TELE_FUNCTION_STATE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, CMD_STEP_MOTOR_SERVICE_DUAL_CAMERA_LINKAGE, STATE_STOPPED);
    wsResNotifyCamFunctionState(CMD_NOTIFY_TELE_FUNCTION_STATE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, CMD_STEP_MOTOR_SERVICE_DUAL_CAMERA_LINKAGE, STATE_IDLE);
    
    return 0;
}

int MotorController::wideViewPixelCoordToMechanicalCoord(const cv::Point& pixel_coord, HorizontalCoordinates& mechanical_coord, const HorizontalCoordinates& center_mechanical_coord) {
    // 像素坐标与三维坐标互转
    const double cam_wide_f_ = 6780.0;// 理论焦距6.78mm，6780um
    const double cam_wide_pix_size_ = 2.8;// 广角像素大小2.8um 
    const double cam_wide_f_pix_ = cam_wide_f_ / cam_wide_pix_size_;// 广角像素焦距

    const double cam_wide_resolution_x_ = 1920;// 与pix_size对应的分辨率
    const double cam_wide_resolution_y_ = 1080;
    const double cam_wide_center_pix_x_ = cam_wide_resolution_x_ / 2.0;
    const double cam_wide_center_pix_y_ = cam_wide_resolution_y_ / 2.0;

    // 像平面在三维空间的x轴上
    const cv::Mat cam_wide_matrix_pixel_to_camera_coord_ = (cv::Mat_<double>(3, 3) << 1.0, 0, 0, 
                                                                                    cam_wide_center_pix_x_, cam_wide_f_pix_, 0, 
                                                                                    cam_wide_center_pix_y_, 0, -cam_wide_f_pix_);

    cv::Mat pixel_coord_vector = (cv::Mat_<double>(3, 1) << 1.0, pixel_coord.x, pixel_coord.y);

    cv::Mat camera_coord_vector;
    camera_coord_vector = cam_wide_matrix_pixel_to_camera_coord_.inv() * pixel_coord_vector;

    // 相机坐标系下，图像边界的方位角和俯仰角都在(-π/2 <= atan() <= π/2)中
    double theta_camera_coord = atan(camera_coord_vector.at<double>(0, 1));
    double phi_camera_coord = atan(camera_coord_vector.at<double>(0, 2) * cos(theta_camera_coord));

    cv::Mat xyz_camera_coord = (cv::Mat_<double>(3, 1) << cos(phi_camera_coord) * cos(theta_camera_coord), 
                                                          cos(phi_camera_coord) * sin(theta_camera_coord), 
                                                          sin(phi_camera_coord));

    cv::Mat matrix_rotate_z = (cv::Mat_<double>(3, 3) << COSD(center_mechanical_coord.azi), SIND(center_mechanical_coord.azi), 0,
                                                        -SIND(center_mechanical_coord.azi), COSD(center_mechanical_coord.azi), 0,
                                                         0, 0, 1);

    cv::Mat matrix_rotate_y = (cv::Mat_<double>(3, 3) << COSD(center_mechanical_coord.alt), 0, SIND(center_mechanical_coord.alt),
                                                         0, 1, 0,
                                                        -SIND(center_mechanical_coord.alt), 0, COSD(center_mechanical_coord.alt));

    cv::Mat matrix_rotate = matrix_rotate_y * matrix_rotate_z;

    cv::Mat xyz_mechanical_coord = matrix_rotate.inv() * xyz_camera_coord;

    double r = cv::norm(xyz_mechanical_coord);//应该为1
    mechanical_coord.alt = RADIANS_TO_DEGREES(asin(xyz_mechanical_coord.at<double>(0, 2) / r));

    mechanical_coord.azi = RADIANS_TO_DEGREES(atan2(xyz_mechanical_coord.at<double>(0, 1), xyz_mechanical_coord.at<double>(0, 0)));
    mechanical_coord.azi = LIMIT_DEGREES_TO_360(mechanical_coord.azi);

    log_i("mechanical_coord:(%f, %f)", mechanical_coord.azi, mechanical_coord.alt);

    return 0;
}